/*
 * File:   FollowSuperState.cpp
 * Author: Mohan
 *
 * Created on 11 juillet 2012, 17:58
 */

#include "FollowSuperState.h"

FollowSuperState::~FollowSuperState() {
}

FollowSuperState::FollowSuperState(const FollowSuperState& s2) {
	m_state = s2.m_state;
	//Array initialization to zero
	for (int s_index = 0; s_index < state_t::size; s_index++) {
		for (int a_index = 0; a_index < action_t::size; a_index++) {
			for (int sp_index = 0; sp_index < state_t::size; sp_index++) {
				m_countTransitions[s_index][a_index][sp_index] = s2.m_countTransitions[s_index][a_index][sp_index];
			}
		}
	}
}

FollowSuperState::FollowSuperState() {
	FollowState::States s;
	s.follow1 = true;
	s.posX = 0;
	s.posY = 0;
	m_state.setValue(s);
	//Array initialization to zero
	for (int s_index = 0; s_index < state_t::size; s_index++) {
		for (int a_index = 0; a_index < action_t::size; a_index++) {
			for (int sp_index = 0; sp_index < state_t::size; sp_index++) {
				m_countTransitions[s_index][a_index][sp_index] = 0;
			}
		}
	}
}

FollowSuperState::FollowSuperState(const state_t& state, int NA1, int N1, int E1, int S1, int W1,
														 int NA2, int N2, int E2, int S2, int W2) {
    m_state = state;

    state_t::States from,to;

    //Set first person
    from.follow1 = true;
    to.follow1 = true;
    //Staying in the same position
    for (int i = -2; i <= 2; i++) {
    	for (int j = -2; j <= 2; j++) {
    		from.posX = i;
    		from.posY = j;
    		to.posX = i;
    		to.posY = j;
    		int f = stateToIndex(from);
    		int t = stateToIndex(to);
    		for (int a_index = 0; a_index < action_t::size; a_index++) {
    			m_countTransitions[f][a_index][t] = NA1;
    		}
    	}
    }
    //Moving north
    for (int i = -2; i <= 2; i++) {
        for (int j = -2; j <= 1; j++) {
        	from.posX = i;
        	from.posY = j;
        	to.posX = i;
        	to.posY = j+1;
        	int f = stateToIndex(from);
        	int t = stateToIndex(to);
        	for (int a_index = 0; a_index < action_t::size; a_index++) {
        		m_countTransitions[f][a_index][t] = N1;
        	}
        }
    }
    //Moving south
    for (int i = -2; i <= 2; i++) {
        for (int j = -1; j <= 2; j++) {
        	from.posX = i;
        	from.posY = j;
        	to.posX = i;
        	to.posY = j-1;
        	int f = stateToIndex(from);
        	int t = stateToIndex(to);
        	for (int a_index = 0; a_index < action_t::size; a_index++) {
        		m_countTransitions[f][a_index][t] = S1;
        	}
        }
    }
    //Moving east
    for (int i = -2; i <= 1; i++) {
        for (int j = -2; j <= 2; j++) {
        	from.posX = i;
        	from.posY = j;
        	to.posX = i+1;
        	to.posY = j;
        	int f = stateToIndex(from);
        	int t = stateToIndex(to);
        	for (int a_index = 0; a_index < action_t::size; a_index++) {
        		m_countTransitions[f][a_index][t] = E1;
        	}
        }
    }
    //Moving west
    for (int i = -1; i <= 2; i++) {
        for (int j = -2; j <= 2; j++) {
        	from.posX = i;
        	from.posY = j;
        	to.posX = i-1;
        	to.posY = j;
        	int f = stateToIndex(from);
        	int t = stateToIndex(to);
        	for (int a_index = 0; a_index < action_t::size; a_index++) {
        		m_countTransitions[f][a_index][t] = W1;
        	}
        }
    }

    //Set second person
    from.follow1 = false;
    to.follow1 = false;
    //Staying in the same position
    for (int i = -2; i <= 2; i++) {
     	for (int j = -2; j <= 2; j++) {
       		from.posX = i;
       		from.posY = j;
       		to.posX = i;
       		to.posY = j;
       		int f = stateToIndex(from);
       		int t = stateToIndex(to);
       		for (int a_index = 0; a_index < action_t::size; a_index++) {
       			m_countTransitions[f][a_index][t] = NA2;
       		}
       	}
    }
    //Moving north
    for (int i = -2; i <= 2; i++) {
        for (int j = -2; j <= 1; j++) {
          	from.posX = i;
           	from.posY = j;
           	to.posX = i;
           	to.posY = j+1;
           	int f = stateToIndex(from);
           	int t = stateToIndex(to);
           	for (int a_index = 0; a_index < action_t::size; a_index++) {
           		m_countTransitions[f][a_index][t] = N2;
           	}
        }
    }
    //Moving south
    for (int i = -2; i <= 2; i++) {
       for (int j = -1; j <= 2; j++) {
          	from.posX = i;
           	from.posY = j;
           	to.posX = i;
           	to.posY = j-1;
           	int f = stateToIndex(from);
           	int t = stateToIndex(to);
           	for (int a_index = 0; a_index < action_t::size; a_index++) {
           		m_countTransitions[f][a_index][t] = S2;
           	}
        }
    }
    //Moving east
    for (int i = -2; i <= 1; i++) {
        for (int j = -2; j <= 2; j++) {
          	from.posX = i;
           	from.posY = j;
           	to.posX = i+1;
           	to.posY = j;
           	int f = stateToIndex(from);
           	int t = stateToIndex(to);
           	for (int a_index = 0; a_index < action_t::size; a_index++) {
           		m_countTransitions[f][a_index][t] = E2;
         	}
        }
    }
    //Moving west
    for (int i = -1; i <= 2; i++) {
       for (int j = -2; j <= 2; j++) {
          	from.posX = i;
           	from.posY = j;
           	to.posX = i-1;
           	to.posY = j;
           	int f = stateToIndex(from);
           	int t = stateToIndex(to);
           	for (int a_index = 0; a_index < action_t::size; a_index++) {
           		m_countTransitions[f][a_index][t] = W2;
           	}
        }
    }

}

FollowSuperState::FollowSuperState(const state_t& state) {
    m_state = state;
    for (int s_index = 0; s_index < state_t::size; s_index++) {
    	for (int a_index = 0; a_index < action_t::size; a_index++) {
    		for (int sp_index = 0; sp_index < state_t::size; sp_index++) {
    			m_countTransitions[s_index][a_index][sp_index] = 0;
    		}
    	}
    }
}

//Overload comparison to be used in a belief
bool FollowSuperState::operator < (const FollowSuperState &s2) const {
    if (m_state.index() < s2.m_state.index()) {
        return true;
    } else if (s2.m_state.index() < m_state.index()) {
        return false;
    } else {
    	for (int s_index = 0; s_index < state_t::size; s_index++) {
    			for (int a_index = 0; a_index < action_t::size; a_index++) {
    				for (int sp_index = 0; sp_index < state_t::size; sp_index++) {
    					if (m_countTransitions[s_index][a_index][sp_index] < s2.m_countTransitions[s_index][a_index][sp_index]) {
    						return true;
    					} else if (s2.m_countTransitions[s_index][a_index][sp_index] < m_countTransitions[s_index][a_index][sp_index]) {
    					    return false;
    					}
    				}
    			}
    		}
        return false;
    }
}

FollowSuperState::state_t FollowSuperState::getState() const {
    return m_state;
}

FollowSuperState FollowSuperState::update(int s, int a, int ss) const {
    FollowSuperState newSuperState(*this);

    newSuperState.m_countTransitions[s][a][ss]++;
    return newSuperState;
}

double FollowSuperState::getT(int s, int a, int ss) const {
    double r = 0;
    for (int i = 0; i < state_t::size; i++) {
    	r += m_countTransitions[s][a][i];
    }
    r = m_countTransitions[s][a][ss] / r;
    return r;
}

double FollowSuperState::getO(int s, int a, int z) const {
    return 0.8;
}

int FollowSuperState::stateToIndex(const state_t::States & s)
{
	int r = s.follow1?0:25;
	r += (s.posX + 2);
	r += (s.posY + 2)*5;
	return r;
}
